perm filename ARM[SYS,HE]2 blob
sn#064726 filedate 1973-10-02 generic text, type T, neo UTF8
COMMENT ⊗ VALID 00046 PAGES
RECORD PAGE DESCRIPTION
00001 00001
00005 00002 TITLE ARM
00007 00003 These are conditional assembly flags
00009 00004 IFN SIMU,<
00013 00005 THIS IS THE 32 WORD MAIL MESSAGE
00014 00006 IFN SIMU,< THIS IS SIMULATION ONLY
00018 00007 SKIPE BAND
00019 00008 MOVE 1,FILE
00020 00009 ZDTH:
00023 00010 BACK:
00025 00011 IFN REPORT+TRACK,<
00026 00012 READ: SETZM STWT
00027 00013 SPACE: MOVEI 1,=180
00029 00014 FINISH: SPCWAR 636367
00030 00015 SETFLT: SKIPN NERVES
00031 00016 IFN USER+TRACK,<0>
00034 00017 NXTJIF:
00036 00018 WATSTL: CONI CLOCK,AC
00038 00019 IFN ADJUST,<
00039 00020 MAIN: TRNE RUN
00042 00021 TT: PUSHJ P,REFLEX
00043 00022 ANGLES: MOVEI I,5
00046 00023 IFE USER,<
00047 00024 SERVO: TRNN RUN
00049 00025 DRIVE:
00052 00026 IFN SIMU,< IF IN SIMU PREDICT NEXT VALUE OF THETA
00054 00027 NXTH: MOVE AC,[XWD T0,T0P]
00057 00028 EVAL: MOVEI I,5
00062 00029 DHO: AOJ I, DISASTER HAS OCCURED
00065 00030 NEXT: TRNE RUN
00069 00031 PREAD:
00071 00032 NON: POINT 12,INNER(AC),11
00076 00033 DYNAMICS
00077 00034 RADIX = 10
00080 00035 HOPEN: OPENS THE HAND TO SET POINT
00083 00036 HCLOSE:
00085 00037 HEAD: MOVEI K,HANDCHA
00087 00038 GOTHER: FSBR AC,TAC HAND SERVO
00088 00039 PLACE: PLACE HAND ON TABLE
00089 00040 NUDGE: HRRE TAC,@STKPTR
00091 00041 PATH: TRO RUN
00093 00042 SAVE: HRRE K,@STKPTR
00096 00043 CENTER: TRNN HCL
00099 00044 SEARCH: SETZM XSEL
00102 00045 REFLEX: MOVEI I,1
00103 00046 XLIST
00104 ENDMK
⊗;
TITLE ARM
; These are the error messages returned by the arm servo program
ALLOK←←0 ;ALL OK
NOFILE←←3 ;FILE NOT FOUND
NODATA←←4 ;ENTER ON DATA FILE FAILED
NODEV←←5 ;DEVICE DSK DOES NOT EXIST
EXER←←1 ;IF JUST 1 THEN OVERFLOW OR UNDERFLOW OCCURED,
;IF OF THE FORM X1 THEN EXCESSIVE FORCE REQUIRED AT JOINT X
GRASER←←2 ;GRASP ERROR, HAND CLOSED TOO FAR
TOUCH←←6 ;STOPPED ON TOUCH
;THIS ERROR IS OF THE FORM X6 WHERE THE X'th SENSOR TOUCHED
READER←←7 ;A/D READ ERROR
TERR←←20 ;SPACEWAR JOB FAILS TO TERMINATE
HTERR←←22 ;HAND FAILS TO FINISH IN TIME
STPERR←←23 ;ARM FAILED TO STOP ON FORCE LIMIT
LOOPER←←24 ;ARM IN CONTROL LOOP
SAVERR←←25 ;SAVE ARRAY INDEX ERROR
FUNERR←←27 ;FUNCTION DOES NOT EXIST
NOTHER←←30 ;THE ARM IS DOWN
INVMES←←40 ;MESSAGE INVALID, ARM NOT IN WAIT STATE
REAERR←←50 ;TRAJECTORY FILE READ ERROR
VERSN←←60 ;VERSION NUMBER WRONG
REFERR←←70 ;REFERENCE SUPPLY IS OFF,CHECK THE FUSE
PDPERR←←100 ;PDP-6 IS DOWN
ISERR←←200 ;PDP-6 SERVICE INTERRUPTED
ADBUS←←300 ;AD IS BUSY
XGPBUS←←400 ;XGP IS BUSY
XLIST
SUBTTL AC DEFINITIONS AND FLAGS LOU PAUL
LIST
; These are conditional assembly flags
DEB←←0 ;causes the AC's to be saved each jiffy
STEP←←0 ;No acceleration compensation and predicted velocity
TACH←←0 ;Integrates tach voltage and sets vgain
TRACK←←0 ;Causes a debugging file to be written
IFE TRACK,<
DYNAMICS←←0 ;CAUSES FORWARD AND FEEDBACK TO BE WRITTEN OUT
REPORT←←1+DYNAMICS ;Causes display data to be written
>
IFN TRACK,<
REPORT←←0
DYNAMICS←←0
>
SIMU←←0 ;Turns this program into a sail callable routine to
;simulate the arm
USER←←0+SIMU ;This runs the SPACEWAR job in user mode and simulates the arm
ADJG←←1 ;READS THE POT BOX TO SET SERVO GAINS
IFN ADJG,<ADJD←←0>
IFE ADJG,<
ADJD←←0 ;READS THE POT BOX TO SET DRIVE FACTORS
>
ADJUST←←ADJG+ADJD ;CAUSES THE POTS TO BE READ TO ADJUST GAINS
HANDWORK←←1 ;IF ZERO DOSN'T DRIVE THE HAND
ARMWORK←←1 ;IF ZERO RETURNS THE ARM DOWN MESSAGE
IFN SIMU,<
INTERNAL ARM.INIT,ARM.STEP
INTERNAL ARM.THETA,ARM.GRASP,ARM.TOUCH,ARM.FUNCTION
>
EXTERNAL JOBREN,JOBOPC
IFN ADJUST,<EXTERNAL EXP>
OPDEF INTUUO [723B8]
OPDEF UFBRD [706B8]
; AC'S
BITS←←0
AC←1
TAC←2
MQ←3
I←5
J←6
K←7
L←10
DACVAL←11 ;CONTAINS THE A/D READING WITH THE SIGNBITS COMP'D
CBUF←12 ;POINTS TO CURRENT BUFFER
DATWD←13 ;ARM DATAO
DATA←14 ;DATA POINTER FOR DEBUGGING BUFFER
TAC1←15
P←17 ;PUSH DOWN POINTER
; CONSTANTS
CHA←←62B23 ;JOINT 6 POT CHANNEL
CHA5←←61B23 ;JOINT 5 POT CHANNEL
N6CHA←←63B23 ;ALT JOINT 6 POT CHANNEL
HANDCHA←←70B23 ;HAND POT CHANNEL
PDLL←←5 ;LENGTH OF PUSH DOWN
MASTER←←16660 ;VERSION NUMBER
DBL←←6000 ;LENGTH OF DEBUGGING BUFFER
AD←←424 ;DEVICE AD
DEVT←←500 ;DEVICE TOOL
DB←←204 ;DEVICE 136
ARM←←420 ;DEVICE ARM
CLOCK←←730 ;DEVICE CLOCK
WIDTH←←600 ;D/A TO SET PULSE WIDTHS
MAXFN←←22 ;MAXIMUM NUMBER OF FUNCTIONS
JMPABS←←100 ;JUMP ABSOLUTE
SKPERR←←101 ;SKIP ON ERROR
SKPNER←←103 ;SKIPN ON ERROR
SKPSER←←104 ;SKIPS ON ERROR
JMPREL←←102 ;JUMP RELATIVE
IFN USER,<OPDEF DATAO [JFCL]>
; FLAGS WHICH RESIDE IN AC 0 OR IN BITSAV
DTST←←1 ;SPACE WAR JOB FINISHED
POTREAD←←2 ;READ POTS IN OCTAL+=1000, DO NOT CONVERT TO DEGREES
RONLY←←4 ;JUST READ POTS
FINAL←←10 ;NULL ARM ERRORS
RUN←←20 ;RUN ARM
INCREM←←40 ;INCREMENT SET POINT TOWARD DTH
DROP←←100 ;PLACE ARM DO NOT STOP IF ERRORS ARE ZERO
ALT6←←200 ;READ ALT POT 6
HCL←←400 ;INITIALIZATION FOR HAND FUNCTIONS
ALT←←1000 ;TRAJECTORY ALT PATH
STP←←2000 ;STOP ON FORCE LIMIT
TRIP←←4000 ;TURN ON ARM
REREAD←←10000 ;AD HAS HAD TO BE RESTARTED
STCH←←20000 ;STOP ON TOUCH
WOB←←100000 ;WOBBLE OUTER JOINTS
NSET←←200000 ;DO CALL SETSET
BOTH←←400000 ;MERGE
;FLAGS WHICH RESIDE IN THE LH OF DATWD
NNUL←←1 ;DO NOT NULL
INT←←2 ;INTERPOLATE CI AS THE ARM MOVES
NXTFN←←4 ;START NEXT FUNCTION NOW
;FLAGS IN LH OF INSTRUCTIONS
MERGE←←400 ;MERGE
RELOCF←←40
HOLDF←←20
DEC←←-4
XF←←1
XALL
XLIST
SUBTTL MESSAGE FORMAT LOU PAUL
LIST
; THIS IS THE 32 WORD MAIL MESSAGE
EXE: XWD MASTER,0 ;18-27 NARGS, 30 RELOC, 31 HOLD, 32 SOTCH, 33-35 EXE
;0 READ IN OCTAL
;1 READ IN DECIMAL
;2 EXECUTE FILE
;3 EXECUTE FUNCTION
FILE: 0
PPPN: 0
BAND: 0
SEGJOB: 0
NAME: 0
STATUS: 0
SEGMENT:0
NERVES: 0
ETOR: 0
THETA: 0
ARGS: BLOCK =21
XLIST
SUBTTL USER LEVEL CODE LOU PAUL
LIST
IFN SIMU,< ;THIS IS SIMULATION ONLY
ARM.IN: SETZM INRUN ;TO CALL GAIN AND CHANGE JOBNAM TO [ARM]
SETOM ARMINI ;INDICATES ARM.IN HAS BEEN CALLED
PUSHJ P,GAIN
ARM.ST: SETO 1, ;INDICATES END OF RUN
SKIPN ARMINI
POPJ P,
MOVEM 16,SSAVE+16 ;SAVE SAIL AC'S EXCEPT FOR STACK
MOVEI 16,SSAVE
BLT 16,SSAVE+15
SKIPE INRUN
JRST[ HRLZI 16,MSAVE ;PUT BACK IN RUN AC'S
BLT 16,15
MOVE 16,MSAVE+16
JRST SSW] ;AND RETURN TO SPACE WAR
JRST GETMES
INRUN: 0 ;IN THE MIDDLE OF A RUN DO NEXT TICK
ARMINI: 0 ;ARM.IN HAS BEEN CALLED
SSAVE: BLOCK 17 ;SAIL AC'S
MSAVE: BLOCK 17 ;USER AC'S
WAIT: HRLZI 16,SSAVE ;PUT BACK AC'S
BLT 16,15
MOVE 16,SSAVE+16
SETZM INRUN
POPJ P,
GETMES: SETOM INRUN
>
IFE SIMU,<
START: MOVE P,[XWD -PDLL,UPDL]
PUSHJ P,GAIN
OUTSTR[ASCIZ/!
/]
WAIT:
>
WRCV EXE
HLRZ 5,EXE
CAIE 5,MASTER
JRST[ MOVEI 3,VERSN
JRST FINISH]
WAITED: MOVE 1,SEGJOB
HRRZM 1,JOB
HLRZM 1,SEG
IFN REPORT+TRACK,<
MOVE 1,[IOWD DBL+100,DATO]
MOVEM 1,DATPT ;DATA POINTER
MOVEM 1,DATIWD ;OUTPUT POINTER
>
SETZB 3,TRAJER
MOVEI 0,0
LDB 1,[POINT 3,EXE,35]
CAIG 1,1
JRST READ
IFE ARMWORK,<
MOVEI 3,NOTHER
JRST FINISH
>
IFN REPORT+TRACK,< ;WRITE OUT DATA
INIT 15,17
SIXBIT /DSK/
0
JRST NDM
MOVE 2,FILE
MOVEM 2,DFILE
MOVE 2,PPPN
MOVEM 2,DPPPN
IFN TRACK,<
SKIPE WALK
JRST[ LOOKUP 15,DFILE
JRST NDMS
MOVE 2,[IOWD DBL,DATO]
MOVEM 2,DATPT
INPUT 15,1
JRST EXFN]
>
ENTER 15,DFILE
JRST[NDMS:MOVEI 3,NODATA
SUB 17,[XWD 1,1]
JRST FINISH]
>
EXFN: CAIN 1,3
JRST[ MOVE 4,EXE
TRNN 4,HOLDF
JRST [ SKIPE ARGS
SOS JUMPBK
JRST MOVON]
SOS 1,STKPTR
MOVEI 2,(1)
LDB 3,[POINT 9,EXE,26]
SUBI 1,(3)
HRLI 1,ARGS
BLT 1,(2)
TRNE 4,RELOCF
JRST[ MOVE 1,STKPTR
SUBI 1,(3)
SUB 1,CBUFS
HRRM 1,@STKPTR
JRST BPTR]
BPTR: SOS STKPTR
MOVON: SETOM FUNCT
JRST SSW]
CAIE 1,2
JRST[ MOVEI 3,INVMES
JRST FINISH]
SETZM FUNCT
HRREI 2,BUF-1001
MOVEM 2,CBUFS
SETOM BUF
SETZM DOING ;TRAJECTORY SET UP FOR
SKIPE BAND
JRST[ MOVEI 1,BUF
MOVEM 1,GOODIE
MOVEI 1,1001
MOVEM 1,GOODIE+1
SETZM GOODIE+2
MOVE 14,BAND
UFBRD 14,GOODIE
JRST RE
SKIPN BUF+1000
JRST[ SETZM BLOCKS
MOVEI 1,BUF2
MOVEM 1,NXTBUF
SETZM @NXTBUF
JRST ZDTH]
MOVEI 1,BUF2
MOVEM 1,GOODIE
MOVEI 1,20
MOVEM 1,GOODIE+2
UFBRD 14,GOODIE
JRST RE
MOVEI 1,=74
SKIPN BUF2+1000
MOVEI 1,0
MOVEM 1,BLOCKS
MOVEI 1,40
MOVEM 1,GOODIE+2
MOVEI 1,BUF
MOVEM 1,NXTBUF
MOVNS BUF2
JRST ZDTH]
MOVE 1,FILE
MOVEM 1,LFILE
MOVE 1,PPPN
MOVEM 1,LPPPN
INIT 16,17
SIXBIT/DSK/
XWD 0,0
JRST[NDM: MOVEI 3,NODEV
JRST FINISH]
LOOKUP 16,LFILE
JRST[ MOVEI 3,NOFILE
JRST FINISH]
MOVS 1,LPPPN ;FILE LENGTH
MOVN 1,1
ASH 1,-11
MOVEM 1,BLOCKS
SOSGE BLOCKS
JRST FINISH
MOVE 10,[IOWD 1000,BUF]
SETZ 11,
IN 16,10
JRST[ SOSGE BLOCKS
JRST[ MOVEI 1,BUF2
MOVEM 1,NXTBUF
SETZM @NXTBUF
JRST ZDTH]
ADDI 10,1001
IN 16,10
JRST[ MOVEI 1,BUF
MOVEM 1,NXTBUF
MOVNS BUF2
JRST ZDTH]
JRST RE]
JRST RE
ZDTH:
MOVE 1,[XWD DELTH,DELTH+1]
SETZM DELTH
BLT 1,DELTH+5
MOVEI 1,=60
MOVEM 1,STWT
MOVNS BUF
SSW:
IFE USER+TRACK,<LOCK 1,>
PUSHJ P,SPACE
WWS:
IFE REPORT+TRACK<
IFE USER<
MOVEI 1,1
CALLI 1,31
>
>
MOVE BITSAV
TRNE DTST
JRST BACK
MOVE 1,@NXTBUF
JUMPGE 1,.+2 ;MORE TRAJECTORY TO COME
JRST[ SOSGE BLOCKS
JRST[ SETZM @NXTBUF ;ALL DONE MAKK THIS AS LAST
RELEASE 16,
JRST WDO]
SKIPE BAND
JRST[ MOVE 1,NXTBUF
MOVEM 1,GOODIE
UFBRD 14,GOODIE
JRST RBD
MOVE 1,NXTBUF
SKIPN 1000(1)
SETZM BLOCKS
MOVEI 1,20
ADDM 1,GOODIE+2
JRST ROK]
MOVE 10,NXTBUF
SOJ 10,
HRLI 10,-1000
IN 16,10
JRST[ROK:MOVE 1,NXTBUF
MOVNS (1)
ADDI 1,1001
CAILE 1,BUF2
MOVEI 1,BUF
MOVEM 1,NXTBUF
JRST WDO]
RBD: MOVE 1,NXTBUF ;READ ERROR
SETZM @NXTBUF ;MARK AS LAST
WFF: MOVE BITSAV ;AND WAIT FOR IT TO END
TRNN DTST
JRST WFF
RE: MOVEI 3,REAERR
JRST BACK]
WDO:
IFN SIMU,<
MOVEM 16,MSAVE+16
MOVEI 16,MSAVE
BLT 16,MSAVE+15
HRLZI 16,SSAVE
BLT 16,15
MOVE 16,SSAVE+16
SETZ 1,
POPJ P,
>
IFE REPORT+TRACK,<
IFE USER,<JRST WWS>
IFN USER,<JRST SSW>
>
IFN TRACK,<
SKIPE WALK
JRST SSW
>
IFN REPORT+TRACK,<
HRRZ 1,DATIWD ;START TO WRITE
CAIL 1,DATO+DBL-1
JRST DOD
HRRZ 2,DATPT ;LAST LOC FILLED
WA: SUBI 2,(1)
JUMPL 2,[MOVEI 2,DATO+DBL-1
JRST WA]
TRZ 2,177
JUMPN 2,[ MOVN 1,2 ;MORE THAN 200 WORDS SO WRITE IT OUT
HRLM 1,DATIWD ;NEG WORD COUNT
OUT 15,DATIWD
HRRZ 1,DATIWD
ADDI 1,(2)
HRRM 1,DATIWD
JRST DOD]
DOD:
IFE USER,<JRST WWS>
IFN USER,<JRST SSW>
>
BACK:
IFE USER+TRACK,<UNLOCK 1,>
SPCWAR 636367
MOVEI 0,0
SKIPE 3,TRAJER
JRST[ IFE DEB+SIMU,<
IFN TRACK,<SKIPN WALK>
TRO RONLY
MOVEM BITSAV
MOVEI 1,1
CALLI 1,31
DRSW: PUSHJ P,SPACE
DRR: MOVE BITSAV
TRNN DTST
IFE USER,<JRST DRR>
IFN USER,<JRST DRSW>
SPCWAR 636367
MOVEI 0,0
PUSHJ P,SETSET
>
JRST NOERR]
IFN SIMU,<PUSHJ P,SETSET>
NOERR: IFE USER,<RELEASE 17,
RELEASE 14,
>
IFN REPORT+TRACK,<
PUSHJ P,CLODAT
CLOSE 15,
>
SKIPN SEG
JRST[ MOVEM 3,STATUS
MOVE 2,[XWD TH,THETA]
BLT 2,THETA+6
MOVE 2,[XWD TORE,ARGS+10]
BLT 2,ARGS+15
MOVE 1,JOB
MOVEM 1,REPLY
SEND REPLY
JRST WAIT
JRST WAIT]
MOVE 2,SEG
CALLI 2,400016
JRST WAIT
HRRZ 4,NAME
MOVE 2,(4)
HLRZ 4,NAME
MOVEM 2,(4)
PUSHJ P,[SETNAM:MOVE 2,THETA
MOVEI 4,6(2)
HRLI 2,TH
IFN TRACK,<SKIPN WALK>
BLT 2,(4)
MOVE 2,ETOR
MOVEI 4,6(2)
HRLI 2,TORE
BLT 2,(4)
PUSHJ P,SETFLT
MOVEM 3,@STATUS
MOVE 4,DOING
MOVEM 4,@SEGMENT
HRRZ 2,NAME
SETZM (2)
HRLZI 2,40
MOVEM 2,IBIT
INTUUO 4,JOB
JUMP
MOVEI 2,0
CORE2 2,
JFCL
POPJ P,]
JRST WAIT
IFN REPORT+TRACK,<
CLODAT:IFN TRACK,<
SKIPE WALK
POPJ P,
>
NWI: HRRZ 1,DATIWD
HRRZ 2,DATPT
SUBI 2,(1)
JUMPL 2,[ HRRZI 2,DATO+DBL-1
SUBI 2,(1)
MOVN 2,2
HRLM 2,DATIWD
SKIPE 2
OUT 15,DATIWD
MOVEI 2,DATO-1
HRRM 2,DATIWD
JRST NWI]
ADDI 2,200
TRZ 2,177
MOVN 1,2
HRLM 1,DATIWD
OUT 15,DATIWD
POPJ P,
>
READ: SETZM STWT
SKIPG 1
TRO POTREAD
TRO RONLY
RSW: PUSHJ P,SPACE
RSWW: MOVE BITSAV
TRNN DTST
IFE USER,<JRST RSWW>
IFN USER,<JRST RSW>
MOVE 3,TRAJER
PUSHJ P,SETSET
JRST FINISH
SPACE: MOVEI 1,=180
MOVEM 1,COUNT
TRO TRIP
LDB 1,[POINT 1,EXE,32]
MOVEM 1,SOTCHS
JUMPE 1,.+2
TRO STCH
IFN DEB,<SETZM ETIM>
MOVEM BITSAV
IFN TRACK,<SKIPE WALK>
IFN TRACK+USER,<
JRST[ MOVEM 17,USAVE+17
MOVEI 17,USAVE
BLT 17,USAVE+16
JSR STRADR-1
HRLZI 17,USAVE
BLT 17,16
MOVE 17,USAVE+17
POPJ P,]
>
IFE USER,<
GETAD: INIT 17,417
SIXBIT /AD/
XWD 0,0
JRST[ POP P,3
MOVEI 3,ADBUS
JRST FINISH]
SPCWAR 1,STRADR
>
POPJ P,
UPDL: FINISH
BLOCK PDLL
IFN USER+TRACK,<
USAVE: BLOCK 20
>
IFN TRACK,<WALK:0>
IFN REPORT+TRACK,<
DFILE: 0
SIXBIT/TMP/
0
DPPPN: 0
>
LFILE: 0
SIXBIT/TRJ/
0
LPPPN: 0
GOODIE: BLOCK 3
FINISH: SPCWAR 636367
IFE USER,<RELEASE 17,
RELEASE 14,
>
SKIPE SEG
JRST [ATSEG: MOVE 1,SEG
CALLI 1,400016
JRST WAIT
PUSHJ P,SETNAM
JRST WAIT]
MOVEM 3,STATUS
MOVE 1,[XWD TH,THETA]
BLT 1,THETA+6
MOVE 1,JOB
MOVEM 1,REPLY
SKIPN 1
JRST WAITED
SEND REPLY
JRST WAIT
JRST WAIT
REPLY: 0
EXE
SEG: 0
SOTCH: 0
JOB: 0
IBIT: 000040000000
TICKS: 0
SETFLT: SKIPN NERVES
POPJ P,
MOVEI 6,1
MOVE 4,NERVES
STL: MOVE 5,OBS(6)
MOVEM 5,(4)
ADDI 4,=16
SOJGE 6,STL
POPJ P,
XLIST
SUBTTL SPACEWAR CODE CONTROL LOU PAUL
LIST
IFN USER+TRACK,<0>
STRADR: MOVE BITSAV ;Enters here every 1/60 th. seccond
IFE USER+TRACK,<
TRNE DTST
CALLI 400024
DATAO ARM,[2]
SOSLE STWT
CALLI 400024
>
MOVE P,[XWD -PDLL,PDL] ;PUSH DOWN STACK
MOVE CBUF,CBUFS
MOVE DATWD,DATWDS
IORI DATWD,2 ;YELLOW ARM
IFN REPORT+TRACK,<
MOVE DATA,DATPT
>
IFN USER+TRACK,<
TRNE DTST
JRST SWDONE
>
IFE USER,<
IFN TRACK,<
SKIPE WALK
JRST NCHK
>
CONSZ 40 ;CHECK TO SEE IF ON PDP-6
JRST[ MOVEI TAC,PDPERR
MOVEM TAC,TRAJER
JRST TOFF]
CONI 0,AC
ANDI AC,7
CONO 0,2050(AC)
>
TRZE TRIP ;TURN ON ARM
JRST[
DATAO ARM,[3]
NCHK: MOVEI K,51B23 ;-10 VOLT REFERENCE
PUSHJ P,PREAD ;REFTR← -10 volt reference
LDB AC,SNUM ;PICK UP MIDDLE SAMPLE
ANDI DACVAL,7777 ;LEAVE LAST SAMPLE IN J
ADDI AC,(DACVAL) ;ADD THEM TOGETHER
CAIGE AC,17000 ;IS THE FUSE BLOWN
JRST[ MOVEI TAC,REFERR
MOVEM TAC,TRAJER
JRST TOFF]
MOVEM AC,REFTR ;REFERENCE READING
MOVEI I,2 ;FIRST THREE TACHS
MOVE K,VCHAN(I)
ZVEL: PUSHJ P,PREAD
LDB AC,SNUM
ANDI DACVAL,7777
ADDI AC,(DACVAL)
MOVEM AC,VZ(I)
SOJGE I,ZVEL
JRST .+1]
IFN TRACK,<
SKIPE WALK
JRST TOK
>
MOVEI AC,=16666
MOVEM AC,ETIME
MOVSI AC,(1.0)
MOVEM AC,TDF
IFE USER,<
CONI CLOCK,AC
TLZ AC,777774
JRST STIME ;TIME IN MICRO SECONDS
>
IFN USER,<JRST .+2>
NXTJIF:
IFN USER,<JRST SWDONE>
IFN DEB<
MOVEM 17,ACSAV+17
MOVEI 17,ACSAV
BLT 17,ACSAV+16
MOVE 17,ACSAV+17
>
IFN TRACK,<
SKIPE WALK
JRST TOK
>
IFN REPORT+TRACK,<
HRRZI TAC,(DATA)
CAIL TAC,DATO+DBL-1
JRST[ SUB DATA,[XWD DBL,DBL]
MOVE TAC,[XWD DATO+DBL,DATO]
BLT TAC,(DATA)
HRRZI AC,(DATA)
HRRZ TAC,DATIWD
SUBI AC,(TAC)
JRST RDP]
HRRZI AC,(DATA)
HRRZ TAC,DATIWD
SUBI AC,(TAC)
HRRZ MQ,DATPT
SUBI TAC,(MQ)
IMUL AC,TAC
RDP: SKIPLE AC
IFN TRACK,<PUSH DATA,[0]>
IFN REPORT,<PUSH DATA,[SIXBIT/ERROR/]>
MOVEI AC,DATO-1
HRRZ TAC,DATIWD
CAIL TAC,DATO+DBL-1
HRRM AC,DATIWD
MOVEM DATA,DATPT
>
IFE USER,<
MOVEI TAC,=2000
WATSTL: CONI CLOCK,AC
TLZ AC,777774
SUB AC,LTIME
JUMPGE AC,.+2
ADD AC,[=1000000]
IFN REPORT,<
CAIN TAC,=2000
JRST[ PUSH DATA,[<SIXBIT/TICK/>+1]
PUSH DATA,AC
JRST .+1]
>
MOVEM AC,ETIME
TRNE RUN
JRST[ MOVE MQ,AC
FSC MQ,233
FMPR MQ,[0.00006]
MOVEM MQ,TDF
MOVE AC,LTIME
ADD AC,ETIME
JRST SSTIME]
CAIGE AC,=16667 ;delta T greater than 16.7 msec.?
SOJGE TAC,WATSTL
JUMPL TAC,TE
MOVSI AC,(1.0)
MOVEM AC,TDF
MOVE AC,LTIME
ADDI AC,=16667
SSTIME: TLNE AC,777774
SUB AC,[=1000000]
STIME: MOVEM AC,LTIME ;LTIME←LTIME+16.7 msec.
>
SOSGE COUNT
JRST[TE:MOVEI AC,TERR
MOVEM AC,TRAJER
JRST ZITO]
IFN ADJUST,<
MOVEI K,14B23
MOVEM BITSAV
MOVEI I,2
RNP: PUSHJ P,PREAD
LDB AC,SNUM
FSC AC,221
JSA 16,EXP
JUMP AC
CAMG [1.2]
SETZ 0,
FMPR [0.20]
MOVEM FACTOR(I)
SOJGE I,RNP
MOVE BITSAV
MOVE I,ADJI
JUMPL I,MAIN
IFN ADJG,<
MOVE AC,ADJV
FMPR AC,FACTOR
MOVEM AC,KV(I) ;POT 12
MOVE AC,ADJE
FMPR AC,FACTOR+1
MOVEM AC,KE(I) ;POT 13
MOVE AC,ADJIF
FMPR AC,FACTOR+2
MOVEM AC,KI(I) ;POT 14
>
IFN ADJD,<
MOVE AC,ADJE
FMPR AC,FACTOR
MOVEM AC,KM(I) ;POT 12
MOVE AC,ADJV
FMPR AC,FACTOR+1
MOVEM AC,PK(I) ;POT 13
>
>
MAIN: TRNE RUN
JRST[ PUSHJ P,EVAL
PUSHJ P,NXTH
JRST .+1]
MOVEI J,6
PUSHJ P,ANGLES ;read joint angles
TRNE RONLY ;read angles only?
JRST[ PUSHJ P,HEAD ;read hand also
MOVE AC,HAND
MOVEM AC,SET
PUSHJ P,REFLEX
PUSHJ P,READED] ;DOES NOT RETURN
TRNN RUN
JRST[ SKIPG TAC,FUNCT ;not run, mabye a function
PUSHJ P,SETSET
JUMPL TAC,NEXT
JUMPE TAC,[CAIL CBUF,BUF;try next buffer
SETOM (CBUF) ;dont do this if first buffer
MOVEM CBUF,CBUFS
ADDI CBUF,1001
CAILE CBUF,BUF2
MOVEI CBUF,BUF ;Advance buffer
MOVE 2,(CBUF)
JUMPL 2,[BUFWT:MOVE CBUF,CBUFS;wait for buffer
JRST SWDONE]
JUMPN 2,BUFRDY ;the last buffer stop.
JRST[ TRO DTST
MOVEI 2,WAITIN
HRRM 2,JUMPBK
MOVEM 2,STKPTR
IFN REPORT,<PUSH DATA,[0]>
JRST BUFWT]
BUFRDY: HRRZ 2,(CBUF)
ADDI 2,-1(CBUF)
HRRZM 2,STKPTR
JRST NEXT]
CAIN TAC,6
JRST SETCH
JRST TT] ;otherwise perform function
JUMPE J,[RUDONE:
TRNN NSET
PUSHJ P,SETSET
TRZ DROP+RUN+FINAL+WOB ;if J is zero then end of run
TDZ DATWD,[770001252500]
TRZE STP
JRST[ MOVEI TAC,STPERR
MOVEM TAC,TRAJER
POP P,TAC
JRST TOFF]
SKIPN AC,FUNCT
JRST NEXT
CAIN AC,22
JRST[ TRZ HCL
SETZM TDAT
SETZM FUNCT
DATAO DEVT,TDAT
JRST NEXT]
MOVEI AC,=16667
MOVEM AC,ETIME
JRST .+1]
TT: PUSHJ P,REFLEX
DISP: MOVE AC,FUNCT
JUMPL AC,.+2
CAILE AC,MAXFN
JRST[ MOVEI TAC,FUNERR
MOVEM TAC,TRAJER
JRST TOFF]
JRST @.+1(AC) ;Perform function
NXTJIF ;0
HOPEN ;1
HCLOSE ;2
NXTJIF ;3
PLACE ;4
NUDGE ;5
SETCH ;6
STOP ;7
SAVE ;10
RESTORE ;11
CENTER ;12
SET.ARM ;13 SET_ARM
WOBBLE ;14
SEARCH ;15
AOJI ;16
SLAVE ;17
HOME ;20
PATH ;21
TOOL ;22
XLIST
SUBTTL SPACEWAR CODE ANGLE MEASUREMENT LOU PAUL
LIST
ANGLES: MOVEI I,5
JFCL 10,.+1
MOVEI L,10
SETCHA: MOVEI K,CHA
TRNE ALT6
MOVEI K,N6CHA
SKIPA
RSTRT: MOVE K,CHAN(I)
SOJL L,REDER
IFE USER,<
PUSHJ P,PREAD
LDB AC,SNUM
CAIN I,5
JRST[ TRNE POTREAD
JRST .+1
CAIL AC,1000
CAILE AC,7000
JRST[ TRC ALT6
JRST SETCHA]
MOVEI K,CHA5
JRST .+1]
ANDI DACVAL,7777
ADDI AC,(DACVAL)
SETZ TAC,
DIV AC,REFTR
ASH AC,-26
MOVE TAC1,AC
TRNE POTREAD
JRST[ ADDI TAC1,=1000
JRST EXON]
ASHC AC,-11 ;LEAVE INDEX INTO NON-LINEAR DIFF TABLE IN AC
LDB MQ,NON(I) ;GET LOWER DIFF
ADDI TAC1,(MQ) ;ADD IT TO READING
AOJ AC,
LDB AC,NON(I) ;GET UPPER DIFFERENCE
SUBI AC,(MQ)
MUL TAC,AC ;INTERPOLATE
ADD TAC1,TAC ;CORRECTED READING
FSC TAC1,226 ;FLOAT
FMPR TAC1,SCALE(I)
FADR TAC1,OFF(I)
>
IFN USER,<MOVE TAC1,TH(I)>
CAIN I,5
JRST[ TRNE ALT6
FADR TAC1,OFF6
FADR TAC1,ROT6
MOVEM TAC1,TH+5
FSBR TAC1,T0+5
MOVSI MQ,(180.0)
CAML TAC1,MQ
JRST[ INCROT: MOVN MQ,MQ
FSC MQ,1
FADRM MQ,ROT6
FADRM MQ,TH+5
JRST THS]
MOVN MQ,MQ
CAMG TAC1,MQ
JRST INCROT
JRST THS]
EXON: MOVEM TAC1,TH(I) ;THETA
THS:
IFN REPORT,<
MOVE MQ,TH(I)
FSBR MQ,T0(I)
SPOS: MOVEM MQ,JER(I)
>
EXCH TAC1,THP(I)
IFE USER,<CAILE I,2>
JRST[ FSBR TAC1,THP(I)
FDVR TAC1,TDF
MOVNM TAC1,TD(I)
JRST SERVO]
IFE USER,<
MOVE K,VCHAN(I)
NVEL: PUSHJ P,PREAD
LDB AC,SNUM
ANDI DACVAL,7777
ADDI AC,(DACVAL)
CAIG AC,17000
CAIG AC,776
JRST[ FSBR TAC1,THP(I)
FDVR TAC1,TDF
MOVNM TAC1,TD(I)
JRST MEAVEL]
SUB AC,VZ(I)
FSC AC,216
FMPR AC,VSCALE(I)
MOVEM AC,TD(I)
MEAVEL:
>
XLIST
SUBTTL SPACEWAR CODE SERVO CALC. LOU PAUL
LIST
SERVO: TRNN RUN
JRST[ SOJGE I,RSTRT
POPJ P,]
SERVL: MOVE TAC,TH(I)
FSBR TAC,T0(I) ;POSITION ERROR
MOVEM TAC,ET0(I)
TRNE FINAL
JRST NULL ;MODIFY THETA BY INTEGRAL TERM
MOVE TAC1,T0(I)
FSBR TAC1,T0P(I)
FDVR TAC1,TDF
MOVE AC,TAC1
FSBR AC,TDP(I)
MOVEM TAC1,TDP(I)
FDVR AC,TDF
MOVEM AC,TDD(I)
FMPR AC,CII(I)
MODT0: HRRZ K,BMASK(I)
TDNN K,FBI
JRST [ PDIR:SETZM ET0(I)
FMPR AC,CII(I)
JRST NOBACK]
FADRM TAC,ERRINT(I)
IFE STEP,<
MOVN TAC,TD(I)
FADR TAC,TAC1
>
IFN STEP,<MOVN TAC,TD(I)>
IFN REPORT,<SVEL:MOVNM TAC,VER(I)>
FMPR TAC,KV(I) ;VELOCITY GAIN
FADR AC,TAC
FMPR AC,CII(I)
MOVN TAC,ET0(I)
FMPR TAC,KE(I) ;ERROR GAIN
FADR AC,TAC
MOVN TAC,ERRINT(I)
FMPR TAC,KI(I)
FDVR TAC,CII(I)
FADR AC,TAC
NOBACK: CAIE I,2
FMPR AC,[IRAD:0.0174532925] ;JOINT 3 IS IN INCHES
MOVEM AC,TORE(I)
FADR AC,CI(I) ;GRAVITY TERM
MOVEM AC,T(I) ;ERROR CORRECTION TORQUE
XLIST
SUBTTL SPACEWAR CODE DRIVE CALC. LOU PAUL
LIST
DRIVE:
LS0: TDNN DATWD,BMASK(I)
JRST ISP
MOVE MQ,TD(I)
JUMPN MQ,.+2
MOVN MQ,ET0(I)
FMPR MQ,T(I)
JUMPL MQ,[ MOVM MQ,T(I)
FSBR MQ,F0(I)
JUMPL MQ,[ FDVR MQ,F0(I)
FMPR MQ,V0(I)
FSC MQ,1
FADR MQ,V0(I)
SKIPGE T(I)
MOVN MQ,MQ
JRST CEMF]
MOVSI AC,(1.0)
FSBR AC,PK(I)
FMPR MQ,AC
FMPR MQ,KM(I)
FADR MQ,V0(I)
SKIPG T(I)
MOVN MQ,MQ
JRST CEMF]
SKIPN T(I)
JRST[ SKIPN TD(I)
JRST CEMF
MOVE MQ,V0(I)
SKIPGE TD(I)
MOVN MQ,MQ
JRST CEMF]
JUMPE MQ,NOEX
MOVE MQ,V0(I)
SKIPGE T(I)
MOVN MQ,MQ
NOEX: MOVSI AC,(1.0)
FADR AC,PK(I)
FMPR AC,T(I)
FMPR AC,KM(I)
FADR MQ,AC
CEMF:
MOVE AC,EMF(I)
FMPR AC,TD(I)
MOVSI TAC1,(30.0)
JUMPGE MQ,DRVLT
MOVN AC,AC
DRVLT: FSBR TAC1,AC ;Available drive voltage
CAMGE TAC1,[1.0]
JRST MAXDRV
VELOK: MOVM AC,MQ
FDVR AC,TAC1 ;AC= REL TIME ON
FIX AC,211000 ;1=16
CAML AC,[4000000]
JRST DHO ;too much force stop the arm
CAILE AC,776000
MAXDRV: MOVEI AC,776000
IFN REPORT+TRACK+USER+DEB,<
MOVEM AC,MOTOR(I)
SKIPGE MQ
MOVNS MOTOR(I)
>
TRC AC,400000
HRL AC,I
SETDRV:
IFN TRACK,<SKIPN WALK>
DATAO WIDTH,AC
TDZ DATWD,DMASK(I)
JUMPGE MQ,ISP
TDO DATWD,DMASK(I)
ISP: SKIPE REV(I)
TDC DATWD,DMASK(I)
DATAO ARM,DATWD
SOJGE I,RSTRT
JFCL 10,DHO
IFN TRACK,<SKIPN WALK>
IFN SIMU,< ;IF IN SIMU PREDICT NEXT VALUE OF THETA
MOVEI I,5
PNT: MOVE AC,T0(I) ;PRESENT VALUE
FSC AC,1 ;X 2
FSBR AC,T0P(I) ;PREVIOUS VALUE
MOVEM AC,TH(I) ;IS NEXT VALUE
SOJGE I,PNT
>
IFN REPORT,< ;OUTPUT ERRORS AND DRIVES
HRLI AC,PBLK
HRRI AC,1(DATA)
ADD DATA,[XWD 25,25]
BLT AC,(DATA)
>
SETZM ASTP
TRNE STP
JRST[ MOVEI I,5
MTL: MOVE MQ,TORE(I)
FDVR MQ,F0(I)
SKIPN TD(I)
FSC MQ,-2
FMPR MQ,STQ(I)
FADRM MQ,ASTP
SOJGE I,MTL
MOVE AC,ASTP
CAML AC,SDTQ
JRST[ AOS AC,ASTPP
CAIL AC,2
JRST[ TRZ STP
SUB P,[XWD 1,1]
SETZM ERRINT
MOVE AC,[XWD ERRINT, ERRINT+1]
BLT AC,ERRINT+5
JRST RUDONE]
POPJ P,]
SETZM ASTPP
POPJ P,]
POPJ P,
NULL: SETZB AC,TAC1
TLNN DATWD,NNUL
TRNE WOB
SOJA J,MODT0
TDNN DATWD,GMASK(I)
SOJA J,DRIVE
MOVE MQ,FBI
TDNN MQ,GMASK(I)
SOJA J,PDIR
MOVM MQ,TAC ;|POSITION ERROR|
CAMLE MQ,ERR(I) ;COMPARE POSITION ERROR
JRST MODT0
MOVM MQ,TD(I)
CAMLE MQ,ERR(I)
JRST MODT0
SO: TDZ DATWD,BMASK(I)
IFN REPORT+TRACK+USER+DEB,<SETZM MOTOR(I)>
DATAO ARM,DATWD
HRLZ MQ,I
TRC MQ,400000
DATAO WIDTH,MQ
SOJA J,DRIVE
POPJ P,
XLIST
SUBTTL SPACEWAR CODE LOU PAUL
LIST
NXTH: MOVE AC,[XWD T0,T0P]
BLT AC,T0P+5
TRNE WOB
JRST[ MOVEI I,2
WL1: SOSGE J,WOBCNT(I)
JRST[ MOVEI J,=19
MOVEM J,WOBCNT(I)
JRST WL2]
WL2: MOVE AC,SIN(J)
FMPR AC,WOBMAG
FADRM AC,TFF+3(I)
SOJGE I,WL1
JRST .+1]
TRNE DROP
JRST[ MOVEI I,5
OL: MOVE AC,DELTH(I)
FMPR AC,TDF
FADRM AC,DTH(I)
SOJGE I,OL
JRST JALS]
NTH:
TRNE INCREM
JRST[ MOVE MQ,ETIME
ADDB MQ,TICKS
CAML MQ,NTICKS
JRST[ TRZ INCREM
SKIPN TP
TRO FINAL
MOVEI AC,=600
MOVEM AC,COUNT
SKIPN TP(I)
TLNN DATWD,INT
JRST FIL1
MOVE AC,[XWD NC,CI]
BLT AC,CII+5
FIL1: MOVEI I,5
FIL: MOVE AC,DELTH(I)
FADRB AC,DTH(I)
FADR AC,TFF(I)
MOVEM AC,T0(I)
SETZM DELTH(I)
SOJGE I,FIL
POPJ P,]
SETZ I,
DIV MQ,NTICKS
ASH MQ,-10
FSC MQ,200
MOVSI AC,(6.0)
FMPR AC,MQ
FADR AC,[-15.0]
FMPR AC,MQ
FADR AC,[10.0]
FMPR AC,MQ
FMPR MQ,MQ
FMPR MQ,AC
MOVEI I,5
LDC: MOVE AC,DELTH(I)
FMPR AC,MQ
FADR AC,DTH(I)
FADR AC,TFF(I)
MOVEM AC,T0(I)
SKIPN TP
TLNN DATWD,INT
JRST LDC1
MOVE AC,DCI(I)
FMPR AC,MQ
FADR AC,PC(I)
MOVEM AC,CI(I)
MOVE AC,DCII(I)
FMPR AC,MQ
FADR AC,PCC(I)
MOVEM AC,CII(I)
LDC1: SOJGE I,LDC
POPJ P,]
JALS: MOVEI I,5
JAL: MOVE AC,DTH(I)
FADR AC,TFF(I)
MOVEM AC,T0(I)
SOJGE I,JAL
POPJ P,
EVAL: MOVEI I,5
EL: MOVE J,TP(I) ;POINTS TO KOE5
JUMPE J,NE
MOVE MQ,ETIME
ADDB MQ,TJ(I)
CAML MQ,TN(I) ;NUMBER OF TICKS THIS SEGMENT
JRST[ ;END OF SEGMENT
CAIN I,5
JRST[ ;JOINT 6 HAS CONTROL WORD
MOVE TAC,TICKS
CAMGE TAC,NTICKS
TRO INCREM
LDB TAC,[POINT 3,1(J),2] ;LEVEL
JUMPN TAC,[ ;SWITCH POINT
SKIPG LOOP(TAC)
JRST[ ;INITIALIZE LOOP
LDB AC,[POINT 15,1(J),17]
MOVEM AC,LOOP(TAC) ;COUNT
JRST SETALT]
SOSLE LOOP(TAC) ;IN LOOP
SETALT: TRO ALT
JRST GETNXT]
JRST GETNXT]
GETNXT: TRNE ALT
JRST[ LDB J,[POINT 9,(J),8] ;ALT POINTER
JRST GOT]
LDB J,[POINT 9,(J),17] ;NEXT POINTER
JUMPE J,[SETZM TP(I)
TRO FINAL
MOVEI AC,=600
MOVEM AC,COUNT
MOVE TAC,[XWD NC,CI]
BLT TAC,CI+=11
MOVE AC,A4(I)
ADD AC,A3(I)
ADD AC,A2(I)
ADD AC,A1(I)
ADD AC,A0(I)
XOR AC,SC(I)
FSC AC,0
MOVEM AC,TFF(I)
JRST NE]
GOT: ADDI J,(CBUF)
MOVEM J,TP(I)
SUB MQ,TN(I) ;MICRO SEC INTO NEW SEGMENT
MOVEM MQ,TJ(I) ;INITIALIZE
HRRZ AC,(J)
ASH AC,16
MOVEM AC,TN(I)
CAIN I,5
JRST[
HRRZ AC,1(J)
ADDI AC,(CBUF)
MOVEI MQ,6(AC)
MOVEI K,5
MOVE MQ,(MQ) ;NEW DATWD
FTF: MOVE TAC,GMASK(K)
TDNN TAC,FBI
JRST[ TDNN TAC,MQ ;WAS FREE
JRST FNF ;STILL FREE
MOVE TAC,TH(K)
MOVEM TAC,T0(K)
FSBR TAC,TFF(K)
MOVEM TAC,DTH(K)
MOVNM TAC,DELTH(K)
MOVE TAC,TN+5
ASH TAC,16
MOVEM TAC,NTICKS
SETZM TICKS
TRO INCREM
JRST FNF]
TDNN TAC,MQ ;NOT FREE
JRST[ MOVE TAC,TDD(K) ;NOW IS FREE
CAIE K,2
FMPR TAC,IRAD
FMPR TAC,CII(K)
FMPR TAC,CII(K)
FSBR TAC,T(K)
MOVNM TAC,NC(K)
JRST FNF]
FNF: SOJGE K,FTF
PUSHJ P,UPDC
JRST DDELC]
DDELC: HLRE AC,-3(J)
MOVEM AC,A4(I)
HRRE AC,-3(J)
MOVEM AC,A3(I)
HLRE AC,-2(J)
MOVEM AC,A2(I)
HRRE AC,-2(J)
MOVEM AC,A1(I)
HLLZ AC,-1(J)
MOVEM AC,SC(I)
HRRE AC,-1(J)
MOVEM AC,A0(I)
MOVE MQ,TJ(I)
JRST ELL]
ELL: DIV MQ,TN(I)
TLNE DATWD,INT
CAIE I,5
JRST ELL1
MOVEI K,=11
HLRE TAC,MQ
FSC TAC,211
DECC: MOVE AC,DCI(K)
FMPR AC,TAC
FADR AC,PC(K)
MOVEM AC,CI(K)
SOJGE K,DECC
ELL1: MOVE AC,A4(I)
MUL AC,MQ
ADD AC,A3(I)
MUL AC,MQ
ADD AC,A2(I)
MUL AC,MQ
ADD AC,A1(I)
MUL AC,MQ
ADD AC,A0(I)
XOR AC,SC(I)
FSC AC,0
MOVEM AC,TFF(I)
CAIN I,5
JRST[ MOVE AC,TFF+5
FSBR AC,T0+5
MOVSI MQ,(180.0)
CAML AC,MQ
JRST[ INCT0: FSC MQ,1
FADRM MQ,T0+5
JRST .+1]
MOVN MQ,MQ
CAMG AC,MQ
JRST INCT0
JRST .+1]
NE: SOJGE I,EL
TRZ ALT
POPJ P,
DHO: AOJ I, ;DISASTER HAS OCCURED
LSH I,3 ;JOINT NUMBER
IORI I,EXER
MOVEM I,TRAJER
POP P,I
ZITO: SETZM ERRINT
MOVE AC,[XWD ERRINT,ERRINT+1]
BLT AC,ERRINT+5
TOFF: MOVE AC,STKPTR
MOVEM AC,ERRSTK
DATAO DEVT,[0]
MOVEI L,1000
TRNE RONLY
JRST READED
TRZ RUN+FINAL+DROP+INCREM+HCL+STP+WOB ;STOP THE ARM
SETZM TP
MOVE AC,[XWD TP,TP+1]
BLT AC,TP+5
PUSHJ P,SETSET
MOVNXT: AOS AC,STKPTR
MOVNX: SOJL L,[ LE:
MOVEI TAC,LOOPER
MOVEM TAC,TRAJER
JRST PW]
HLRE TAC,@STKPTR
CAIN TAC,3 ;IS IT A WAIT
JRST MOVNXT
CAIN TAC,SKPERR
JRST[ HRRZ TAC,(AC)
CAMN TAC,TRAJER
JRST[ AOS STKPTR
JRST NEXT]
JRST MOVNXT]
CAIN TAC,SKPNER
JRST[ HRRZ TAC,(AC)
CAME TAC,TRAJER
JRST[ AOS STKPTR
JRST MOVNXT]
JRST NEXT]
CAIN TAC,SKPSER
JRST[ LDB MQ,[POINT 3,(AC),35]
LDB TAC,[POINT 3,TRAJER,35]
CAIN TAC,(MQ)
JRST[ HRRZ TAC,(AC)
ANDI TAC,777770
TDNE TAC,TRAJER
JRST[ AOS STKPTR
JRST NEXT]
JRST MOVNXT]
JRST MOVNXT]
CAIN TAC,JMPREL
JRST[ HRRE TAC,(AC)
ADDI AC,(TAC)
HRRM AC,STKPTR
JRST MOVNX]
PW: CAIL AC,BUF
HRRM AC,JUMPBK
MOVEI AC,WAITIN
MOVEM AC,STKPTR
MOVEI AC,3
MOVEM AC,FUNCT
IFN TRACK,<SKIPN WALK>
DATAO ARM,[2] ;STOP THE ARM
IFN REPORT,<PUSH DATA,[0]>
READED: TRO DTST
IFN DEB,<
MOVEM 17,ACSAV+17
MOVEI 17,ACSAV
BLT 17,ACSAV+16
>
JRST SWDONE
NEXT: TRNE RUN
JRST[ SETZM FUNCT
JRST NXTJIF]
MOVEI L,1000
MOVE AC,FUNCT
CAIE AC,6 ;SET TOUCH
JRST[ MOVE TAC,SOTCHS
MOVEM TAC,SOTCH
TRZ STCH
JUMPE TAC,.+1
TRO STCH
JRST .+1]
IFN TRACK,<SKIPN WALK>
MOVEI DATWD,2
DATAO ARM,DATWD ;STOP THE ARM
INCPIC: AOS AC,STKPTR
PICKUP: SOJL L,LE
HLRE TAC,@STKPTR
CAIN TAC,JMPABS
JRST[JMPA:HRRZ TAC,@STKPTR
MOVEM TAC,STKPTR
HLRZ TAC,@STKPTR
CAIN TAC,3
JRST INCPIC
JRST PICKUP]
CAIN TAC,SKPERR
JRST[ HRRZ TAC,@STKPTR
CAMN TAC,TRAJER
AOS STKPTR
JRST INCPIC]
CAIN TAC,SKPNER
JRST[ HRRZ TAC,@STKPTR
CAME TAC,TRAJER
AOS STKPTR
JRST INCPIC]
CAIN TAC,SKPSER
JRST[ LDB MQ,[POINT 3,@STKPTR,35]
LDB TAC,[POINT 3,TRAJER,35]
CAIN TAC,(MQ)
JRST[ HRRZ TAC,@STKPTR
ANDI TAC,777770
TDNE TAC,TRAJER
AOS STKPTR
JRST INCPIC]
JRST INCPIC]
CAIN TAC,JMPREL
JRST[ HRRE TAC,@STKPTR
ADD TAC,STKPTR
HRRM TAC,STKPTR
JRST PICKUP]
SETZM TRAJER
MOVEI I,100
MOVEM I,COUNT
TRZE TAC,MERGE
TRO BOTH
MOVEM TAC,FUNCT
CAIN TAC,3 ;IS IT A WAIT
JRST[ MOVE TAC,STKPTR
CAIL TAC,BUF
HRRM TAC,JUMPBK
MOVEI TAC,WAITIN
MOVEM TAC,STKPTR
IFN REPORT,<PUSH DATA,[0]>
TRO DTST
PUSHJ P,SWDONE]
MARK:
IFN REPORT,<
PUSH DATA,[<SIXBIT/NEXT/>+1]
MOVE TAC,FUNCT
HRL TAC,0
PUSH DATA,TAC
>
; JRST DISP
SWDONE:
MOVEM BITSAV
MOVEM DATWD,DATWDS
MOVEM CBUF,CBUFS
IFN REPORT+TRACK,<
MOVEM DATA,DATPT
>
IFE USER,<
IFN TRACK,<
SKIPE WALK
JRST @STRADR-1
>
CALLI 400024
>
IFN USER,<
JRST @STRADR-1
>
SETSET: MOVEI I,5
TCHK: MOVE J,TP(I)
JUMPE J,ATEND
SETZM TJ(I)
SETZM TN(I)
GNN: LDB J,[POINT 9,(J),17]
ADDI J,(CBUF)
CAIN J,(CBUF)
JRST[ MOVEM J,TP(I)
JRST ATEND]
JRST GNN
ATEND: SOJGE I,TCHK
PUSHJ P,EVAL
MOVEI I,5
FDTH: MOVE AC,TH(I)
MOVEM AC,T0(I)
FSBR AC,TFF(I)
MOVEM AC,DTH(I)
SETZM DELTH(I)
SETZM TDP(I)
SETZM TDD(I)
SOJGE I,FDTH
POPJ P,
UPDC: MOVE TAC,[XWD NC,PC]
BLT TAC,PCC+5
HRL AC,AC
HRRI AC,NC
BLT AC,NC+6
MOVE DATWD,NC+6
TLNE DATWD,NXTFN
JRST[ AOS AC,STKPTR
HLRE AC,(AC)
MOVEM AC,FUNCT
JRST .+1]
HRRZM DATWD,FBI
MOVEI K,5
UPL: MOVE AC,NC(K)
HLLZM AC,NC(K)
HRLZM AC,NCC(K)
MOVE AC,MOTARM(K)
FADRB AC,NCC(K)
TLNN DATWD,INT
JRST UPL1
FSBR AC,PCC(K)
MOVEM AC,DCII(K)
MOVE AC,NC(K)
FSBR AC,PC(K)
MOVEM AC,DCI(K)
UPL1: TDNE DATWD,BMASK(K)
TDO DATWD,BMASK(K)
SOJGE K,UPL
TLNE DATWD,INT
POPJ P,
MOVE AC,[XWD NC,CI]
BLT AC,CII+5
POPJ P,
PREAD:
IFN TRACK,<
SKIPE WALK
JRST[GNPR:AOBJP DATA,[ MOVE AC,[IOWD DBL,DATO]
MOVEM AC,DATPT
MOVE DATA,DATPT
SETZ TAC,
INPUT 15,1
JRST GNPR]
MOVE DACVAL,(DATA)
JRST @READFN]
>
MOVEI TAC,10
MOVEM TAC,DATMSD
DACRED: SOSGE DATMSD
JRST[REDER: MOVEI TAC,READER
MOVEM TAC,TRAJER
JRST TOFF]
DACST: CONO DB,4250
CONO AD,(K)
NREAD: MOVEI TAC,30
CONSO DB,1000
SOJGE TAC,.-1
JUMPL TAC,[DACDIE:CONO AD,4000(K) ;STOP DAC
TRO REREAD
IFN DEB,<MOVEM DACVAL,DACERR>
MOVEI TAC,12
WRH: SOJGE TAC,WRH
JRST DACRED]
DATAI DB,DACVAL
CONSZ DB,10000
JRST DACDIE
XOR DACVAL,SBMSK
IFN TRACK,<PUSH DATA,DACVAL>
JRST @READFN
FREAD: ADDI K,770000
IFN TRACK,<SKIPN WALK>
CONO AD,4000(K) ;STOP AD ON NEXT CHANNEL
POPJ P,
READFN: FREAD
XLIST
SUBTTL CONSTANTS LOU PAUL
LIST
NON: POINT 12,INNER(AC),11
POINT 12,INNER(AC),23
POINT 12,INNER(AC),35
POINT 12,OUTER(AC),11
POINT 12,OUTER(AC),23
POINT 12,OUTER(AC),35
ERR: 0.05
0.05
0.01
0.1
0.1
0.7
IFN SIMU,<
TH-1
ARM.TH: .+4
>
TH: -180.0 ;THE NEXT 7 LOCATIONS ARE BLOCK TRANSFERED.
-90.0
12.0
-90.0
90.0
0.0
IFN SIMU,<ARM.GR:>
HAND: 3.0
TDD: BLOCK 6
ERRINT: 0
0
0
0
0
0
DTH: BLOCK 6
TD: BLOCK 6
T0P: BLOCK 6
TDP: BLOCK 6
ET0: BLOCK 6
T0: -150.0
-75.0
14.0
-90.0
60.0
0.0000
TFF: -150.0
-75.0
14.0
-90.0
60.0
0.0000
DELTH: BLOCK 6 ;THIS AND THE NEXT ARE BLT ED TOGETHER
DDTH: 0
0
0
0
0
0
NTICKS: 0
DTHS: BLOCK =30
TORE: BLOCK 6
T: BLOCK 6
IFN REPORT,<
PBLK: <SIXBIT/THETA/>+6
JER: BLOCK 6
<SIXBIT/VEL/>+6
VER: BLOCK 6
<SIXBIT/DAC/>+6
>
IFN USER+REPORT+TRACK+DEB,<MOTOR: BLOCK 6>
FBI: 252502
VZ: BLOCK 3
IFN TACH,<
VDTH: BLOCK 3
>
THP: BLOCK 6
MOTARM: 840.0
900.0
70.0
55.0
55.0
64.0
STQ: BLOCK 6
SDTQ: 0
ASTP: 0
ASTPP: 0
PC: BLOCK 6
PCC: BLOCK 6
DCI: BLOCK 6
DCII: BLOCK 6
CI: 0
0
0
0
0
0
CII: 1000.0
1200.0
100.0
100.0
100.0
100.0
NC: 0
0
0
0
0
0
NCC: 1000.0
1200.0
100.0
100.0
100.0
100.0
IFN DEB,<DACERR: 0>
REFTR: 0
ETIME: =16667
TDF: 1.0
IFN DEB,<
ETIM: 0
ACSAV: BLOCK 20
>
PDL: DHO
BLOCK PDLL
LTIME: 0
IFN SIMU,<ARM.FU:>
FUNCT: 0
ROT6: 0
TRAJER: 0
DOING: 0
DATMSD: 0
CBUFS: 0
DATWDS: 0
SNUM: POINT 12,DACVAL,23
SBMSK: 400040004000
REV: 0
0
-1
0
-1
-1
DMASK: 400000
100000
20000
4000
1000
200
BMASK: XWD 400000,200000
XWD 200000, 40000
XWD 100000, 10000
XWD 40000, 2000
XWD 20000, 400
XWD 10000, 100
GMASK: 200000
40000
10000
2000
400
100
VCHAN: 52B23
53B23
54B23
CHAN: 55B23
56B23
57B23
60B23
61B23
GAIN: MOVEI I,5
SETGN: MOVE AC,KV(I)
FMPR AC,AC
FMPR AC,THIRD
MOVEM AC,KE(I)
FMPR AC,KV(I)
FMPR AC,SIXTH
MOVEM AC,KI(I)
SOJGE I,SETGN
SETNM: MOVE 1,[SIXBIT/[ARM]/]
CALLI 1,400002
MOVE I,RET
MOVEM I,SETNM
JRST SETNM
RET: POPJ P,
THIRD: 0.3
SIXTH: 0.1
BLOCK =20
0
WAITIN: XWD 3,0
JUMPBK: XWD JMPABS,WAITIN
STKPTR: WAITIN
ERRSTK: 0
BITSAV: 0
COUNT: 0
STWT: 0
HCNT: 0
TP: 0
0
0
0
0
0
TJ: BLOCK 6
LOOP: BLOCK 10
TN: BLOCK 6
SC: BLOCK 6
A0: BLOCK 6
A1: BLOCK 6
A2: BLOCK 6
A3: BLOCK 6
A4: BLOCK 6
BLOCKS: 0
NXTBUF: 0
BUF: 0
BLOCK 1000
BUF2: 0
BLOCK 1000
IFN ADJUST<
ADJI: -1
ADJV: 0
ADJE: 0
ADJIF: 0
FACTOR: BLOCK 3
>
IFN REPORT+TRACK,<
DATIWD: 0
0
DATPT: 0
DATO: BLOCK DBL+100
>
IFN DYNAMICS,<
DDAT: <SIXBIT/FORD/>+6
FOR: BLOCK 6
<SIXBIT/BACK/>+6
BAK: BLOCK 6
>
;DYNAMICS
PK: 0.00033
0.00063
-0.026
-0.0009
0.0
0.06666
F0: 1000.0
520.0
96.0
180.0
330.0
150.0
V0: 1.3
0.7
5.0
6.0
6.5
5.0
KM: 0.00187
0.00191
0.0822
0.02543
0.035
0.06667
EMF: 4.35
4.40
33.0
1.22
1.22
2.92
KV: 600.0
700.0
60.0
100.0
100.0
80.0
KE: BLOCK 6
KI: BLOCK 6
VSCALE: -13.3
-10.8
-1.02
XLIST
SUBTTL ARM CALIBRATION LOU PAUL
LIST
RADIX = 10
INNER: BYTE (12) 995, 979,1020
BYTE (12) 995, 979,1016
BYTE (12) 995, 979,1013
BYTE (12) 995, 982,1010
BYTE (12) 995, 994, 989
BYTE (12) 991,1006, 999
BYTE (12) 989,1021,1001
BYTE (12) 1007,1031,1011
BYTE (12) 1007,1008, 996
BYTE (12) 1010, 983, 973
BYTE (12) 1007, 972, 970
BYTE (12) 1001, 972, 975
BYTE (12) 995, 983, 941
BYTE (12) 988,1008, 944
BYTE (12) 986,1017, 989
BYTE (12) 986,1017,1047
BYTE (12) 986,1017,1073
OUTER: BYTE (12) 1025,1061,1098
BYTE (12) 1019,1061,1098
BYTE (12) 1005,1061,1059
BYTE (12) 979,1061,1014
BYTE (12) 989,1047, 999
BYTE (12) 996,1014, 980
BYTE (12) 1007, 991, 962
BYTE (12) 1013, 973, 947
BYTE (12) 1004, 966, 948
BYTE (12) 997, 959, 955
BYTE (12) 997, 948, 961
BYTE (12) 997, 951, 969
BYTE (12) 997,1006, 988
BYTE (12) 997,1066, 995
BYTE (12) 997,1097,1015
BYTE (12) 997,1097,1063
BYTE (12) 997,1097,1087
RADIX =8
SCALE: 1.233472
0.847226
0.092519
1.341662
-1.120682
-1.408665
OFF: -299.690800
-240.15
2.677699
-234.6
192.2
258.0
OFF6: -183.10
XLIST
SUBTTL HAND FUNCTIONS LOU PAUL
LIST
HOPEN: ;OPENS THE HAND TO SET POINT
TRNN HCL
JRST[ MOVEI TAC,=480 ;GO HERE FIRST TIME
MOVEM TAC,HCNT ;MAX TIME TO SERVO THERE
ADDI TAC,=60
MOVEM TAC,COUNT
HRLZ AC,@STKPTR ;REQUIRED OPENING
MOVE MQ,[3.6]
CAML AC,MQ
MOVE AC,MQ ;NOT GREATER THAN 2.5
MOVEM AC,RSET ;REQUIRED FINAL SET POINT
IFE USER,< PUSHJ P,HEAD ;READS THE HAND>
MOVE AC,HAND ;AND STORE IT HERE
MOVEM AC,SET ;DYNAMIC SET POINT
TRZ STCH ;TURN OFF TOUCH AND SAVE STATE
MOVE TAC,[0.2]
CAML AC,RSET
MOVN TAC,TAC
MOVEM TAC,DIR ;CHANGE OF SET POINT PER JIFFY
TRO HCL ;SO THAT WE DON'T COME BACK HERE
JUMPGE TAC,NXTJIF
FSC TAC,-1 ;IF CLOSING GO SLOWER
MOVEM TAC,DIR
SKIPE SOTCH ;AND RESTORE TOUCH
TRO STCH
JRST NXTJIF]
HSU: PUSHJ P, HEAD ;INCREMENTS SETPOINT AND SETS D/A
;LEAVES ARM DATAO IN TAC
IFE HANDWORK <
JRST NEXT
>
SOSGE HCNT
JRST[ MOVEI AC,HTERR
MOVEM AC,TRAJER
POP P,AC
JRST TOFF]
SKIPE DIR
JRST DAW ;IF NOT AT FINAL SET DRIVE ANYWAY
MOVE AC,RSET
FSBR AC,HAND ;CHECK ERROR TOLERANCE
MOVM AC,AC
FSBR AC,[0.05]
JUMPLE AC,[ TRZ HCL
TDZ DATWD,HANDAT
JRST NEXT]
DAW:
IFN TRACK,<SKIPN WALK>
DATAO ARM,DATWD ;DRIVE HAND
JRST NXTJIF
HCLOSE:
TRNN HCL ;ONLY ONCE
JRST[ TRZ STCH ;TURN OFF TOUCH AND SAVE STATE
MOVEI TAC,=240
MOVEM TAC,COUNT
HRLZ AC,@STKPTR ;MINIMUN OPENING
MOVEM AC,MINDST
IFE USER,< PUSHJ P,HEAD ;READ HAND>
MOVE AC,HAND
MOVEM AC,SET
MOVE TAC,[-0.10]
MOVEM TAC,DIR ;CHANGE OF SET POINT PER JIFFY
MOVSI AC,(4.0)
MOVNM AC,RSET ;FINAL SET POINT -1 SO AS TO SQUEEZE
TRO HCL
JRST NXTJIF]
BB: PUSHJ P,HEAD
IFE HANDWORK <
JRST ER2
>
IFN TRACK,<SKIPN WALK>
DATAO ARM,DATWD ;DRIVE HAND
MOVE AC,HERR
CAMG AC,[1.00]
JRST NXTJIF ;IF ERROR LESS THAN 1 KEEP ON
ER2: TRZ HCL
TDZ DATWD,HANDAT
SKIPE SOTCH ;RESTORE TOUCH
TRO STCH
MOVE AC,MINDST
CAMG AC,HAND
JRST NEXT
MOVEI 1,GRASER ;IF LESS THAN MINOPENING GIVE ERROR
MOVEM 1,TRAJER
POP P,1
JRST TOFF
HEAD: MOVEI K,HANDCHA
IFE USER,<
PUSHJ P,PREAD ;READ HAND CHANNEL
LDB AC,SNUM
ANDI DACVAL,7777
ADDI AC,(DACVAL) ;ADD ALL THREE READINGS TOGETHER
FSC AC,220
FMPR AC,HSCALE
FADR AC,HOFF
MOVEM AC,HAND ;OPENING IN INCHES
>
IFN USER,<
MOVE AC,FUNCT
CAIN AC,1
JRST[ SKIPN DIR
JRST[ MOVE AC,RSET
JRST SFH]
MOVE AC,HAND
FADR AC,DIR
JRST SFH]
MOVE AC,HAND
FADR AC,DIR
CAMG AC,MINDST
MOVE AC,MINDST
SFH: MOVEM AC,HAND
>
IFN REPORT,<
PUSH DATA,[<SIXBIT/HAND/>+1]
PUSH DATA,HAND
>
SKIPN TAC,DIR
JRST[ MOVE TAC,SET
JRST GOTHER]
FMPR TAC,TDF
FADRB TAC,SET ;INCREMENT SET POINT
FSBR TAC,RSET ;NEGATIVE REMAINS TO GO
FMPR TAC,DIR
JUMPGE TAC,[ MOVE TAC,RSET ;GOT THERE
MOVEM TAC,SET
SETZM DIR
JRST GOTHER]
MOVE TAC,SET
GOTHER: FSBR AC,TAC ;HAND SERVO
EXCH AC,HERR
FSBR AC,HERR
FMPR AC,KVH
MOVN TAC,HERR
FMPR TAC,KEH
FADR AC,TAC
TDO DATWD,[HANDAT:XWD 4000,60]
JUMPGE AC,.+2
TRZ DATWD,40
MOVM AC,AC
FIX AC,211000
CAILE AC,776000
MOVEI AC,776000
TRC AC,400000
HRLI AC,6
IFN DEB<
MOVEM AC,HANDAW
>
IFN TRACK,<SKIPN WALK>
DATAO WIDTH,AC
POPJ P,
HSCALE: 1.32
HOFF: -0.81
DIR: 0 ;CHANGE OF SETPOINT PER JIFFY
SET: 0 ;DYNAMIC SET POINT
RSET: 0 ;FINAL SET POINT
MINDST: 0 ;MIN OPENING FOR CLOSE
HERR: 0 ;HAND POSITION ERROR
KEH: 5.0 ;ERROR GAIN
KVH: 10.0 ;VELOCITY GAIN
IFN DEB<
HANDAW: 0
>
PLACE: ;PLACE HAND ON TABLE
TRNN DROP ;ONLY COME HERE ONCE
JRST[ TRO DROP+RUN
MOVEI 1,=360
MOVEM 1,COUNT
HRRE 1,@STKPTR
ADDI 1,(CBUF)
HRLI 1,(1)
HRRI 1,DELTH
BLT 1,DELTH+5 ;CHANGE OF THETA PER JIFFY
MOVEI 1,=12
MOVEM 1,HCNT ;DON'T STOP FOR THE FIRST 12 JIFFIES
MOVE DATWD,[770000252502]
HRRZM DATWD,FBI
JRST NXTJIF]
SOSL HCNT
JRST NXTJIF
IFN USER,<JRST PLACED>
EXL: MOVM AC,ET0+1
CAML AC,PLER
JRST PLACED
JRST NXTJIF
PLACED: SETZM FUNCT
JRST RUDONE
PLER: 0.6
NUDGE: HRRE TAC,@STKPTR
ADDI TAC,5(CBUF)
MOVEI I,5
NUL1: MOVE AC,(TAC)
MOVEM AC,DELTH(I)
TRNE NSET
JRST[ FADRM AC,TFF(I)
MOVN AC,AC
FADRM AC,DTH(I)
JRST .+1]
SOJ TAC,
SOJGE I,NUL1
TRZ NSET
HRRE 1,@STKPTR
ADDI 1,6(CBUF)
PUSHJ P,UPDC
HRRE 1,@STKPTR
ADDI 1,6(CBUF)
MOVE 2,7(1)
ASH 2,16
MOVEM 2,NTICKS
ASH 2,-15
ADDI 2,=60
MOVEM 2,COUNT
TRO INCREM+RUN
SETZM TICKS
TRZE BOTH
JRST[ AOS AC,STKPTR
HLRE AC,(AC)
MOVEM AC,FUNCT
JRST .+2]
SETZM FUNCT
JRST NXTJIF
STOP: HRRE 1,@STKPTR
ADDI 1,(CBUF)
HRLI 1,(1)
HRRI 1,STQ
BLT 1,STQ+5
MOVEI I,5
SETZ AC,
DSTQ: MOVE TAC,STQ(I)
FDVR TAC,F0(I)
MOVM MQ,TAC
CAMGE MQ,[0.2]
SETZB TAC,STQ(I)
MOVEM TAC,STQ(I)
FMPR TAC,TAC
FADR AC,TAC
SOJGE I,DSTQ
MOVEM AC,SDTQ
SETZM ASTPP
TRO STP
JRST NEXT
TOOL: TRNN HCL
JRST[ HRRE TAC,@STKPTR
MOVM AC,TAC
CAIL AC,100
MOVEI AC,77
JUMPGE TAC,TDIP
TRC AC,177
TDIP: TRO AC,200
HRRM AC,TDAT
TRO HCL
JRST .+1]
SKIPG COUNT
JRST[ SETZM TDAT
DATAO DEVT,TDAT
TRZ HCL
JRST NEXT]
DATAO DEVT,TDAT
JRST NXTJIF
TDAT: 0
PATH: TRO RUN
AOS DOING ;INCREASE TRAJECTORY NUMBER
MOVEI AC,=1800
MOVEM AC,COUNT
SETZM LOOP
MOVE TAC,[XWD LOOP,LOOP+1]
BLT TAC,LOOP+9
MOVEI I,5
HRRE K,@STKPTR
ADDI K,34
SUL: SUBI K,4
HRLZM K,TJ(I)
HRLZM K,TN(I)
MOVEI TAC,TJ(I)
MOVEM TAC,TP(I)
SOJGE I,SUL
SETZM ETIME
PUSHJ P,EVAL
TRZ INCREM
HRRE TAC,@STKPTR
ADDI TAC,(CBUF)
HRRZ AC,(TAC)
ASH AC,16
MOVEM AC,NTICKS ;RUN TIME
SETZM TICKS
MOVEI I,5
MOVBK: MOVE AC,TH(I)
MOVEM AC,T0(I)
FSBR AC,TFF(I)
CAIN I,5
JRST[ MOVSI MQ,(180.0)
CAML AC,MQ
JRST[SETROT:FSC MQ,1
MOVN MQ,MQ
FADRM MQ,TH+5
FADRM MQ,ROT6
JRST MOVBK]
MOVN MQ,MQ
CAMG AC,MQ
JRST SETROT
JRST ADDTF]
ADDTF: MOVEM AC,DTH(I)
MOVN AC,AC
FADR AC,DDTH(I)
SETZM DDTH(I)
MOVEM AC,DELTH(I)
SETZM TD(I)
SOJGE I,MOVBK
SETZM TICKS
TRZE BOTH
JRST[ AOS AC,STKPTR
HLRE AC,(AC)
MOVEM AC,FUNCT
JRST .+2]
SETZM FUNCT
JRST NXTJIF
SAVE: HRRE K,@STKPTR
ADDI K,(CBUF)
MOVE TAC,(K)
JUMPLE TAC,.+2
CAILE TAC,12
JRST[ MOVEI TAC,SAVERR
MOVEM TAC,TRAJER
JRST TOFF]
IMULI TAC,3
ADDI TAC,DTHS
MOVEI J,2
SAL1: MOVEI I,5
SOJ TAC,
SETZ AC,
SAL2: AOJ K,
MOVE MQ,DTH(I)
FMPR MQ,(K)
FADR AC,MQ
SOJGE I,SAL2
MOVEM AC,(TAC)
SOJGE J,SAL1
JRST NEXT
RESTORE:HRRE K,@STKPTR
ADDI K,(CBUF)
MOVE TAC,(K)
JUMPLE TAC,.+2
CAILE TAC,12
JRST[ MOVEI TAC,SAVERR
MOVEM TAC,TRAJER
JRST TOFF]
IMULI TAC,3
ADDI TAC,DTHS-1
ADDI K,22
MOVEI I,5
REL1: MOVE AC,(TAC)
FMPR AC,(K)
MOVE MQ,-1(TAC)
FMPR MQ,-1(K)
FADR AC,MQ
MOVE MQ,-2(TAC)
FMPR MQ,-2(K)
FADR AC,MQ
FADRM AC,DDTH(I)
SUBI K,3
SOJGE I,REL1
JRST NEXT
TIMFAC: 0.3
0.6
3.0
0.3
0.3
0.2
SET.ARM:HRRE I,@STKPTR
ADDI I,(CBUF)
HRRE AC,(I)
SOJL AC,.+2
CAIL AC,11
JRST[ MOVEI TAC,SAVERR
MOVEM TAC,TRAJER
JRST TOFF]
IMULI AC,3
HRRI TAC,DTHS(AC)
HRRZI MQ,2(TAC)
HRLI TAC,1(I)
BLT TAC,(MQ)
JRST NEXT
SLAVE: MOVEI K,15B23
MOVEI I,3
SLRP: PUSHJ P,PREAD
LDB AC,SNUM
SUBI AC,4000
MOVEM AC,POT(I)
SOJGE I,SLRP
TRNN HCL
JRST[ TRO RUN
MOVE DATWD,[770000252502]
HRRZM DATWD,FBI
TRO HCL
MOVEI AC,=6000
MOVEM AC,COUNT
MOVEI I,3
SLZL: MOVE AC,POT(I)
MOVEM AC,POTZ(I)
SOJGE I,SLZL
JRST .+1]
MOVEI I,3
SLCH: MOVE AC,POT(I)
SUB AC,POTZ(I)
JUMPGE AC,[SUBI AC,400
JUMPGE AC,SLST
JRST SLZA]
ADDI AC,400
JUMPL AC,SLST
SLZA: SETZ AC,
SLST: FSC AC,213
MOVEM AC,POT(I)
SOJGE I,SLCH
SKIPE POT+3
JRST[ TRZ HCL
TRO FINAL
SETZM FUNCT
JRST NXTJIF]
HRRE K,@STKPTR
ADDI K,21(CBUF)
MOVEI I,5
SLL2: MOVE AC,POT+2
FMPR AC,(K)
MOVE MQ,POT+1
FMPR MQ,-1(K)
FADR AC,MQ
MOVE MQ,POT
FMPR MQ,-2(K)
FADR AC,MQ
FMPR AC,TDF
FADRM AC,DTH(I)
SUBI K,3
SOJGE I,SLL2
JRST NXTJIF
POT: BLOCK 4
POTZ: BLOCK 4
CENTER: TRNN HCL
JRST[ TRO HCL
MOVEI TAC,=600
MOVEM TAC,COUNT
HRLZ AC,@STKPTR ;MINIMUN OPENING
MOVEM AC,MINDST
MOVSI AC,(4.0)
MOVNM AC,RSET
IFE USER,< PUSHJ P,HEAD>
MOVE AC,HAND
MOVEM AC,SET
TRZ STCH
MOVE TAC,[-0.02]
MOVEM TAC,DIR
JRST NXTJIF]
PUSHJ P,HEAD
IFE HANDWORK,<JRST ER2>
DATAO ARM,DATWD
TRNN DROP
JRST[ MOVEI I,1
CFT: MOVE AC,OBS(I)
JUMPG AC,[SETZM TOT
SKIPN I
AOS TOT
TRO DROP+RUN
MOVE TAC,[-0.01]
MOVEM TAC,DIR
HRRE TAC,@STKPTR
ADDI TAC,(CBUF)
MOVE AC,(TAC)
MOVEM AC,MINDST
AOJ TAC,
MOVEI I,DELTH
OT: MOVE AC,(TAC)
FMPR AC,[0.005]
SKIPN TOT
MOVNS AC
MOVEM AC,(I)
AOJ TAC,
CAIGE I,DELTH+5
AOJA I,OT
MOVE TAC,[XWD 770000, 252500]
HRRZM TAC,FBI
IOR DATWD,TAC
MOVSI AC,(1.0)
MOVEM AC,TDF
MOVEI I,4
GETGO: PUSHJ P,NXTH
SOJGE I,GETGO
JRST NXTJIF]
SOJGE I,CFT
JRST NXTJIF]
MOVE TAC,TOT
SKIPG OBS(TAC)
JRST NXTJIF
TRZ DROP+RUN+FINAL
PUSHJ P,SETSET
TDZ DATWD,[XWD 770000,252500]
MOVE TAC,DIR
FSC TAC,3
MOVEM TAC,DIR
MOVEI TAC,2
MOVEM TAC,FUNCT
JRST NXTJIF]
TOT: 0
WOBBLE: HRLZ AC,@STKPTR
MOVEM AC,WOBMAG
MOVEI AC,1
MOVEM AC,WOBCNT+2
MOVEM AC,WOBCNT+1
MOVEI AC,6
MOVEM AC,WOBCNT
MOVN AC,WOBMAG
FADRM AC,DTH+3
TRO WOB
JRST NEXT
WOBMAG: 0
WOBCNT: BLOCK 3
SIN: 0.0
0.30902
0.58779
0.80902
0.95106
1.0
0.95106
0.80902
0.58779
0.30902
0.0
-0.30902
-0.58779
-0.80902
-0.95106
-1.0
-0.95106
-0.80902
-0.58779
-0.30902
SEARCH: SETZM XSEL
SETZM SCOUNT
SETZM TCOUNT
HRRE 1,@STKPTR
ADDI 1,(CBUF)
HRLI 1,(1)
HRRI 1,XDEL
BLT 1,YDEL+5
SETZM SDTH
MOVE AC,[XWD SDTH,SDTH+1]
BLT AC,SDTH+5
JRST NEXT
AOJI: MOVEI I,5
MOVE K,SCOUNT
SOSG J,TCOUNT
JRST[ SKIPN XSEL
AOS K,SCOUNT
SETCMM XSEL
MOVEI J,(K)
MOVEM J,TCOUNT
JRST .+1]
SKIPE XSEL
JRST[IX:MOVE AC,XDEL(I)
TRNN K,1
MOVN AC,AC
FADRB AC,SDTH(I)
FADRM AC,DDTH(I)
SOJGE I,IX
JRST NJ]
IY: MOVE AC,YDEL(I)
TRNN K,1
MOVN AC,AC
FADRB AC,SDTH(I)
FADRM AC,DDTH(I)
SOJGE I,IY
NJ: HRRE TAC,@STKPTR
ADD TAC,STKPTR
SOJ TAC,
HRRM TAC,STKPTR
JRST NEXT
TCOUNT: 0
SCOUNT: 0
XSEL: 0
XDEL: BLOCK 6
YDEL: BLOCK 6
SDTH: BLOCK 6
HOME: HRRE TAC,@STKPTR
ADDI TAC,5(CBUF)
MOVEI I,5
HLN: MOVE AC,(TAC)
FADR AC,6(TAC)
MOVEM AC,TFF(I)
FSBR AC,T0(I)
MOVNM AC,DTH(I)
FADR AC,DDTH(I)
SETZM DDTH(I)
MOVEM AC,DELTH(I)
SOJ TAC,
SOJGE I,HLN
MOVEI I,5
MOVE TAC,DELTH+5
CAML TAC,[180.0]
JRST[ FSBR TAC,[360.0]
JRST .-1]
CAMGE TAC,[-180.0]
JRST[ FADR TAC,[360.0]
JRST .-1]
MOVEM TAC,DELTH+5
HRRE AC,@STKPTR
ADDI AC,=12(CBUF)
PUSHJ P,UPDC
HRRE AC,@STKPTR
ADDI AC,=12(CBUF)
MOVE TAC,7(AC)
ASH TAC,16
MOVEM TAC,NTICKS
MOVEI I,5
SETZ MQ,
HNCCA: MOVM TAC1,DELTH(I)
FMPR TAC1,TIMFAC(I)
CAML TAC1,MQ
MOVE MQ,TAC1
SOJGE I,HNCCA
FADR MQ,[20.0]
FIX MQ,215000
CAML MQ,NTICKS
MOVEM MQ,NTICKS
ASH MQ,-15
ADDI MQ,=60
MOVEM MQ,COUNT
TRO INCREM+RUN+NSET
SETZM TICKS
MOVE AC,STKPTR
TRNE BOTH
AOJ AC,
HLRZ AC,1(AC)
CAIE AC,5
TRZ NSET
TRZE BOTH
JRST[ AOS AC,STKPTR
HLRE AC,(AC)
MOVEM AC,FUNCT
JRST .+2]
SETZM FUNCT
JRST NXTJIF
REFLEX: MOVEI I,1
MOVEI K,65B23
REX:
IFE USER,<
PUSHJ P,PREAD
LDB AC,[POINT 12,DACVAL,11]
ADD AC,REF(I)
MOVEM AC,OBS(I)
>
RMOR: SOJGE I,REX
TRNN STCH
POPJ P,
MOVEI I,1
SIT: MOVE AC,OBS(I)
JUMPG AC,[ LSH I,3
IORI I,TOUCH
MOVEM I,TRAJER
POP P,I
JRST TOFF]
SOJGE I,SIT
POPJ P,
REF: -1000
-1000
BLOCK 4
IFN SIMU,<
OBS-1
ARM.TO: .+4
>
OBS: 0
0
SOTCHS: 0
SETCH: MOVE AC,SOTCH
MOVEM AC,SOTCHS
HRLE AC,@STKPTR
MOVEM AC,SOTCH
JUMPE AC,NEXT
TRO STCH
JRST NEXT
XLIST
LIT
LIST
IFE SIMU,<END START>
IFN SIMU,<END>